#include "ppid.h"
#include "main.h"

void LimitMax(float *value, float max_value)
{
    if(*value > max_value)
        *value = max_value;
    else if(*value < -max_value)
        *value = -max_value;
}

float PID_cal(PID_typedef *PID1, float measure, float target)
{
    if(PID1 == NULL)
        return 0;
    PID1->error[2] = PID1->error[1];
    PID1->error[1] = PID1->error[0];
    PID1->measure = measure;
    PID1->target = target;
		PID1->error[0] = target - measure;
    //LimitMax(&PID->Iout, PID->Max_iout);
    //if (PID->error[0] < 0)
        //PID->error[0] =  -PID->error[0];
    //else
       // return PID->error[0];
		//if(PID->error[0] < PID->Ki_divider)
		{
        PID1->Iout += PID1->Ki * PID1->error[0];
    }
    //else
		{
        //float fast_Iout = PID->fast_Ki * (PID->error[0] - PID->error[1]);
        //LimitMax(&fast_Iout, PID->Max_fast_iout);
        //PID->Iout += fast_Iout;
    }
    LimitMax(&PID1->Iout, PID1->Max_iout);
    PID1->D_item = (PID1->error[0] - PID1->error[1]);
    PID1->Dout = PID1->Kd * PID1->D_item;
    LimitMax(&PID1->Iout, PID1->Max_out);
    PID1->OUT = PID1->Kp * PID1->error[0] + PID1->Iout + PID1->Dout;
    LimitMax(&PID1->OUT, PID1->Max_out);
    return PID1->OUT;
}

float PID_rpm(PID_typedef *PID1, float measure, float target)
{
    if(PID1 == NULL)
        return 0;
    PID1->error[2] = PID1->error[1];
    PID1->error[1] = PID1->error[0];
    PID1->measure = measure;
    PID1->target = target;
		PID1->error[0] = target - measure;
    
		PID1->Iout += PID1->Ki * PID1->error[0];
    
		LimitMax(&PID1->Iout, PID1->Max_iout);
    PID1->D_item = (PID1->error[0] - PID1->error[1]);
    PID1->Dout = PID1->Kd * PID1->D_item;
    LimitMax(&PID1->Iout, PID1->Max_out);
    PID1->OUT = PID1->Kp * PID1->error[0] + PID1->Iout + PID1->Dout;
    LimitMax(&PID1->OUT, PID1->Max_out);
    return PID1->OUT;
}
